#if __STDC_VERSION__ >= 199901L
    #define _XOPEN_SOURCE 600
#else
    #define _XOPEN_SOURCE 500
#endif


#include <stdint.h>     /* C99 types */
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>    /* POSIX terminal control definitions */
#include <sys/ioctl.h>
#include <pthread.h>
#include <time.h>
#include <sys/select.h>
#include <sys/time.h>
#include <poll.h>

#include <stdbool.h>

#include "config.h"
#include "current.h"


#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))
#if DEBUG_CUR == 1
#define DEBUG_MSG(str)                fprintf(stderr,"%s:%d: "str, __FUNCTION__, __LINE__)
#define DEBUG_PRINTF(fmt, args...)    fprintf(stderr,"%s:%d: "fmt, __FUNCTION__, __LINE__, args)
#define CHECK_NULL(a)                if(a==NULL){fprintf(stderr,"%s:%d: ERROR: NULL POINTER AS ARGUMENT\n", __FUNCTION__, __LINE__);return -1;}
#else
#define DEBUG_MSG(str)
#define DEBUG_PRINTF(fmt, args...)
#define CHECK_NULL(a)                if(a==NULL){return -1;}
#endif





#define CMD_TX_SIZE 4 /* id + len_msb + len_lsb + address */
#define CMD_RX_SIZE 8 
#define CMD_RX_DATA_SIZE 4 


#define TX_HEADER     0x88
#define TX_FIX        0xAE
#define CMD_SINGLE    0x11
#define CMD_CONTINUS  0x21
#define CMD_STOP      0x01



void *com_target;


/* -------------------------------------------------------------------------- */
/* --- PRIVATE SHARED VARIABLES (GLOBAL) ------------------------------------ */

pthread_mutex_t mx_ad_uart = PTHREAD_MUTEX_INITIALIZER;

/* -------------------------------------------------------------------------- */
/* --- PRIVATE FUNCTIONS ---------------------------------------------------- */

int set_interface_attribs(int fd, int speed) {
    struct termios tty;

    memset(&tty, 0, sizeof tty);

    /* Get current attributes */
    if (tcgetattr(fd, &tty) != 0) {
        DEBUG_PRINTF("ERROR: tcgetattr failed with %d - %s", errno, strerror(errno));
        return -1;
    }

    cfsetospeed(&tty, speed);
    cfsetispeed(&tty, speed);

    /* Control Modes */
    tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; /* set 8-bit characters */
    tty.c_cflag |= CLOCAL;                      /* local connection, no modem control */
    tty.c_cflag |= CREAD;                       /* enable receiving characters */
    tty.c_cflag &= ~PARENB;                     /* no parity */
    tty.c_cflag &= ~CSTOPB;                     /* one stop bit */
    /* Input Modes */
    tty.c_iflag &= ~IGNBRK;
    tty.c_iflag &= ~(IXON | IXOFF | IXANY | ICRNL);
    /* Output Modes */
    tty.c_oflag &= ~IGNBRK;
    tty.c_oflag &= ~(IXON | IXOFF | IXANY | ICRNL);
    /* Local Modes */
    tty.c_lflag = 0;
    /* Settings for non-canonical mode */
    tty.c_cc[VMIN] = 0;                         /* non-blocking mode */
    tty.c_cc[VTIME] = 50;                       /* wait for (n * 0.1) seconds before returning */

    /* Set attributes */
    if (tcsetattr(fd, TCSANOW, &tty) != 0) {
        DEBUG_PRINTF("ERROR: tcsetattr failed with %d - %s", errno, strerror(errno));
        return -1;
    }

    return 0;
}

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

/* configure TTYACM0 read blocking or not*/
int set_blocking(int fd, bool blocking) {
    struct termios tty;

    memset(&tty, 0, sizeof tty);

    /* Get current attributes */
    if (tcgetattr(fd, &tty) != 0) {
        DEBUG_PRINTF("ERROR: tcgetattr failed with %d - %s", errno, strerror(errno));
        return -1;
    }

    tty.c_cc[VMIN] = (blocking == true) ? 1 : 0;    /* set blocking or non-blocking mode */
    tty.c_cc[VTIME] = 1;                            /* wait for (n * 0.1) seconds before returning */

    /* Set attributes */
    if (tcsetattr(fd, TCSANOW, &tty) != 0) {
        DEBUG_PRINTF("ERROR: tcsetattr failed with %d - %s", errno, strerror(errno));
        return -1;
    }

    return 0;
}



int com_send_cmd(com_cmd_t cmd, int handle) {
    int i;
    int Tlen;
    uint8_t buffertx[CMD_TX_SIZE];

    /* Initialize buffer */
    memset(buffertx, 0, sizeof buffertx);

    /* Prepare command */
    buffertx[0] = cmd.header;
    buffertx[1] = cmd.fix;
    buffertx[2] = cmd.address;
    buffertx[3] = cmd.cmd;
    Tlen = CMD_TX_SIZE;

    /* Send command */
    i = write(handle, buffertx, Tlen);
    if (i < 0) {
        DEBUG_PRINTF("ERROR: failed to write cmd (%d - %s)\n", errno, strerror(errno));
        return -1;
    }

    //DEBUG_PRINTF("Note: sent cmd \'%c\', addr 0x%02X, length=%d\n", cmd.cmd, cmd.address, Tlen);

    return 0;
}



int com_receive_ans(com_ans_t *ans, int handle) {
    int i;
    uint8_t bufferrx[CMD_RX_SIZE];
    int buffer_idx;
    size_t cmd_size = 0;
    int ready;
    fd_set rfds;
    struct timeval tv;

    //struct timespec fetch_time;
    //char fetch_timestamp[30];
    //struct tm * x;


    /* Initialize variables */
    memset(bufferrx, 0, sizeof bufferrx);

    /* Wait for cmd answer header */
    buffer_idx = 0;
    cmd_size = CMD_RX_SIZE;
    while ((bufferrx[0] != 0xFA) ||(bufferrx[1] != 0xFB) || buffer_idx < CMD_RX_SIZE) {
        tv.tv_sec = 0;
        tv.tv_usec = 500000; //500ms tmo
        FD_ZERO(&rfds);
        FD_SET(handle, &rfds);
        ready = select(handle + 1, &rfds, NULL, NULL, &tv);
        if(ready == -1){
            DEBUG_PRINTF("ERROR: select AD error (%d - %s)\n", errno, strerror(errno));
            return -1;
        }
        else if(ready == 0){
            DEBUG_MSG("WARNING: select AD timeout \n");
            return  1;
        }
        else if(!FD_ISSET(handle, &rfds)){
            DEBUG_MSG("WARNING: not expected events \n");
            return -1;
        }
        
        /* Read the answer */
        i = read(handle, &bufferrx[buffer_idx], cmd_size - buffer_idx);
        if (i < 0) {
            DEBUG_PRINTF("ERROR: failed to read cmd answer (%d - %s)\n", errno, strerror(errno));
            return -1;
        }
        buffer_idx += i;
    }

    ans->header = (char)bufferrx[0];
    ans->fix = bufferrx[1];
    ans->address = bufferrx[2];
    ans->verify = bufferrx[CMD_RX_SIZE - 1];
    for (i = 0; i < CMD_RX_DATA_SIZE; i++) {
        ans->data.a[i] = bufferrx[CMD_RX_SIZE - 2 - i];
    }
    
    //clock_gettime(CLOCK_REALTIME, &fetch_time);
    //x = gmtime(&(fetch_time.tv_sec));
    //sprintf(fetch_timestamp, "%04i-%02i-%02i %02i:%02i:%02i.%03liZ", (x->tm_year) + 1900, (x->tm_mon) + 1, x->tm_mday, x->tm_hour + 8, x->tm_min, x->tm_sec, (fetch_time.tv_nsec) / 1000000); /* ISO 8601 format */
    //fprintf(stdout, "%s ++", fetch_timestamp);

    DEBUG_PRINTF("received len : %d \n", buffer_idx);
    DEBUG_PRINTF("received : %2x %2x %2x %4f %2x \n", ans->header, ans->fix, ans->address, ans->data.data_rx , ans->verify);
    return 0;
}


/* -------------------------------------------------------------------------- */
/* --- PUBLIC FUNCTIONS DEFINITION ------------------------------------------ */

int hal_current_com_open(void **com_target_ptr, const char *com_path) {

    int *usb_device = NULL;
    char portname[50];
    int x;
    int fd;
    
    /*check input variables*/
    //CHECK_NULL(com_target_ptr);

    usb_device = malloc(sizeof(int));
    if (usb_device == NULL) {
        DEBUG_MSG("ERROR : MALLOC FAIL\n");
        return -1;
    }

    /* open tty port */
    sprintf(portname, "%s", com_path);
    fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC | O_NDELAY);
    if (fd < 0) {
        printf("ERROR: failed to open COM port %s - %s\n", portname, strerror(errno));
    } else {
        x = set_interface_attribs(fd, B9600);
        x |= set_blocking(fd, false); // true --> false
        if (x != 0) {
            printf("ERROR: failed to configure COM port %s\n", portname);
            free(usb_device);
            return -1;
        }

        *usb_device = fd;
        *com_target_ptr = (void*)usb_device;

        return 0;
    }

    free(usb_device);
    return -1;
}

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

int hal_current_com_close(void *com_target) {
    int usb_device;
    int a;

    /*check input variables*/
   // CHECK_NULL(com_target);

    /* close file & deallocate file descriptor */
    usb_device = *(int*)com_target;
    a = close(usb_device);
    free(com_target);

    /* determine return code */
    if (a < 0) {
        printf("ERROR: failed to close COM port - %s\n", strerror(errno));
        return -1;
    } else {
        DEBUG_MSG("Note : USB port closed\n");
        return 0;
    }
}




int hal_current_read_single(int fd, float *data)  {
    //int fd;
    com_cmd_t cmd;
    com_ans_t ans;

    /* check input variables */

    //CHECK_NULL(data);
    
    cmd.header = TX_HEADER;
    cmd.fix = TX_FIX;
    cmd.address = 0;
    cmd.cmd = CMD_SINGLE;

    pthread_mutex_lock(&mx_ad_uart);
    com_send_cmd(cmd, fd);
    if (com_receive_ans(&ans, fd) == -1) {
        pthread_mutex_unlock(&mx_ad_uart);
        return -1;
    }
    pthread_mutex_unlock(&mx_ad_uart);

    *data = ans.data.data_rx;

    return 0;
}





int hal_current_read_continuous(int fd, int cnt, float *data)  {
    //int fd;
    int i = 0;
    com_cmd_t cmd;
    com_ans_t ans;
    float tmp[1024] = {0};
    float sum = 0;
    int ret = -1;
    int index;
    int tmo = 0;
    int count = 0;
    /* check input variables */

    //CHECK_NULL(data);
    if(cnt > (int)(sizeof(tmp)/sizeof(tmp[0]))){
        cnt = (int)(sizeof(tmp)/sizeof(tmp[0]));
    }

    cmd.header = TX_HEADER;
    cmd.fix = TX_FIX;
    cmd.address = 0;
    cmd.cmd = CMD_CONTINUS;
    
    pthread_mutex_lock(&mx_ad_uart);
    com_send_cmd(cmd, fd);
    do{
        if(i > 0){
             pthread_mutex_lock(&mx_ad_uart);
         }
        ret = com_receive_ans(&ans, fd);
        i++;
        if (ret == -1) {
            cmd.cmd = CMD_STOP;
            com_send_cmd(cmd, fd);
            pthread_mutex_unlock(&mx_ad_uart);
            return -1;
        }else if(ret == 1){
            pthread_mutex_unlock(&mx_ad_uart);
            if(tmo++ < 3)
                continue;
            else
                break;
        }
        tmo = 0;
        pthread_mutex_unlock(&mx_ad_uart);
        tmp[count] = ans.data.data_rx;
        count++;
    }while(i < cnt);

    cmd.cmd = CMD_STOP;
    pthread_mutex_lock(&mx_ad_uart);
    com_send_cmd(cmd, fd);
    pthread_mutex_unlock(&mx_ad_uart);
    for(index = 0; index < count; index++){
         sum += tmp[index];
    }
    DEBUG_PRINTF("sample : %d \n", count);
    if(count > 0)
        *data = sum/count;
    else
        *data = 0;

    return 0;
}



int hal_current_read_stop(int fd)  {
    //int fd;
    com_cmd_t cmd;

    /* check input variables */
    
    cmd.header = TX_HEADER;
    cmd.fix = TX_FIX;
    cmd.address = 0;
    cmd.cmd = CMD_STOP;

    pthread_mutex_lock(&mx_ad_uart);
    com_send_cmd(cmd, fd);
    pthread_mutex_unlock(&mx_ad_uart);

    return 0;
}


#if 0

pthread_t cmd_thrid;
pthread_mutex_t cmd_mx = PTHREAD_MUTEX_INITIALIZER;

char cmd = 'q';
int fd;
float current= 0;
int cnt = 0;

void pthread_scan(void *arg)
{
    char input;
    
    printf("input your select :");
    printf(" 1 : single read \n");
    printf(" 2 : continus read \n");
    printf(" 3 : read stop \n");
    printf(" q : exit \n");

    while(1){
        scanf("%s", &input);
        switch(input){
            case '1':
                current_read_single(fd, &current);
                printf("current = %4f \n", current);
                break;
            case '2':
                printf("input get current count : ");
                scanf("%d", &cnt);
                pthread_mutex_lock(&cmd_mx);
                cmd = input;
                pthread_mutex_unlock(&cmd_mx);
            case '3':
                current_read_stop(fd);
                break;
            case 'q':
            default:
                break;
        }
    }
}



int main(void)
{
    char path[] = "/dev/ttyUSB0";
    void *target;
    
    
    if(-1 == com_open(&target, path)){
        printf("com open error \n");
        return -1;
    }
    fd = *(int *)target;

    pthread_create(&cmd_thrid, NULL, (void *)pthread_scan, NULL);
    while(1){
        if(cmd == '2'){
            pthread_mutex_lock(&cmd_mx);
            cmd = 'q';
            pthread_mutex_unlock(&cmd_mx);
            current_read_continuous(fd, cnt, &current);
            printf("current = %4f \n", current);
        }
    }
}

#endif

